#pragma once
#include "auxiliary.h"
#include "Body.h"
#include "Joint.h"
using namespace Eigen;
using namespace std;

class ForceBase
{
public:
	enum
	{
		ACTIVEFORCE=0x01,
		ACTIVETORQUE=0x02,
		GRAVITY=0x03,
	};
	ForceBase() {};
	~ForceBase() = default;
	virtual VectorXd CalForce(double t, double* y, double* dy);
	virtual VectorXd CalTorque(double t, double* y, double* dy);
	virtual inline unsigned int Type()const = 0;
	virtual int Bi_Id()const = 0;
	virtual bool Write(OUT Json::Value& force)const = 0;
};

class Unary :public ForceBase
{
protected:
	Body* Bi;                                             //target body
	Vector3d rho;                                         //action point on the body
public:
	Unary(Body* B_ptr,const Vector3d& loc) :Bi(B_ptr),rho(loc) {};
	virtual int Bi_Id()const;
	~Unary() {};
};

class ActiveForce :public Unary
{
protected:
	Vector3d F;
	bool adhesion;                                       //if attached to the body
public:
	ActiveForce(Body* Bi_ptr, const Vector3d& loc,const Vector3d& f, bool b = false);
	~ActiveForce() {};
	virtual VectorXd CalForce(double t, double* y, double* dy);
	virtual VectorXd CalTorque(double t, double* y, double* dy);
	virtual inline unsigned int Type()const;
	virtual bool Write(OUT Json::Value& force)const;
};
class Gravity :public ActiveForce
{
	static double g;
public:
	Gravity(Body* Bi_ptr);
	~Gravity() = default;
	virtual inline unsigned int Type()const;
	virtual bool Write(OUT Json::Value& force)const;
};
class ActiveTorque :public Unary
{
private:
	Vector3d M;
	bool adhesion;
public:
	ActiveTorque(Body* Bi_ptr, const Vector3d& m, bool b = false);
	~ActiveTorque() {};
	virtual VectorXd CalTorque(double t, double* y, double* dy);
	virtual inline unsigned int Type()const;
	virtual bool Write(OUT Json::Value& force)const;
};

class JointForce
{
private:
	JointBase* Hj;
	VectorXd Ft;
public:
	friend class MBSystem;
	JointForce(JointBase* j,const VectorXd& ft);
	VectorXd CalForce(double t);
	virtual inline unsigned int DOF()const;
	~JointForce() = default;
	virtual bool Write(OUT Json::Value& force)const;
};